#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>


typedef struct QUEUE Queue;
typedef _Bool Maze;
typedef int ElementType;
struct QUEUE{
	ElementType Elm;
	Queue *Next;
	Queue *Tail;
};

Queue *QueueInit();
_Bool QueueEmpty(Queue*);
_Bool QueueFull(void);
void  EnQueue(Queue*,ElementType);
ElementType DeQueue(Queue*);
void  DestroyQueue(Queue*);
int CoordinateToIndex(int x,int y,int n);

//	With Head Node;
Queue *QueueInit(){
	Queue *q=malloc(sizeof(Queue));
	q->Next=NULL;
	q->Tail=q;
	return q;
}
_Bool QueueEmpty(Queue *q){
	if( q->Next )
		return false;
	return true;
}
_Bool QueueFull(void){
	Queue *q=malloc(sizeof(Queue));
	if( !q )return true;
	free(q);
	return false;
}
void  EnQueue(Queue* q,ElementType x){
	Queue *new=malloc(sizeof(Queue));
	new->Elm=x;
	new->Next=NULL;
	q->Tail->Next=new;
	q->Tail=new;
	return;
}
ElementType DeQueue(Queue* q){
	ElementType ret=q->Next->Elm;
	Queue *tmp=q->Next;
	q->Next=tmp->Next;
	free(tmp);
	if( !(q->Next) )
		q->Tail=q;
	return ret;
}
void  DestroyQueue(Queue* q){
	Queue *Next;
	do{
		Next=q->Next;
		free(q);
		q=Next;
	}while( q );
	return ;
}
int CoordinateToIndex(int x,int y,int n){
	//n+=2;
	return n*(x-1)+y;
}
void IndexToCoordinate(int index,int n,int *x,int *y){
	(*x)=index/n+1;
	(*y)=index%n;
	return;
}
void BFS(int n,const Maze (*)[n+2],int *,int start_x,int start_y);

#define MSIZE 5
#define NODES ((MSIZE*MSIZE)+1)
int main(void){
	const Maze maze[MSIZE+2][MSIZE+2]=
	{	{0,0,0,0,0,0,0},
		{0,1,1,0,0,0,0},
		{0,0,0,1,0,0,0},
		{0,0,0,1,0,0,0},
		{0,0,0,0,1,0,0},
		{0,0,0,0,1,0,0},
		{0,0,0,0,0,0,0},
	};
	int   path[NODES];
	//Queue *q=QueueInit();
	//for(int i=0;i<20;i++)EnQueue(q,i+30);
	//while( !QueueEmpty(q) )printf("%d ",DeQueue(q));
	//DestroyQueue(q);
	//printf("Pos( 0,0 ) to Arr Index%d\n",CoordinateToIndex(0,0,MSIZE));
	//int x,y;
	//IndexToCoordinate(-4,MSIZE,&x,&y);
	//printf("Arr Index -4 to Pos(%d,%d)\n",x,y);
	BFS(MSIZE,maze,path,1,1);
	for(int i=1;i<NODES;i++)printf("%3d",i);printf("\n");
	for(int i=1;i<NODES;i++)printf("%3d",path[i]);printf("\n");
	int x,y;
	int target=24;
	while( -1 != path[target] ){
		IndexToCoordinate(path[target],MSIZE,&x,&y);
		printf("Pos(%d,%d)\n",x,y);
		target=path[target];
	}
	return 0;
}
//int CoordinateToIndex(int x,int y,int n){
//void IndexToCoordinate(int index,int n,int *x,int *y){
void BFS(int n,const Maze (*maze)[n+2],int *path,int s_x,int s_y){
printf("Start BFS\n");
	int nodes=n*n+1;
	_Bool Visited[nodes];
	for(int i=1;i<nodes;i++){
		Visited[i]=false;
		path[i]=-1;
	}
	int x,y;
	int currentIndex=CoordinateToIndex(s_x,s_y,n);
	int s;
	Queue *q=QueueInit();
	EnQueue(q,currentIndex);
	Visited[currentIndex]=true;
	while( !QueueEmpty(q) ){
		currentIndex=DeQueue(q);
		IndexToCoordinate(currentIndex,n,&x,&y);
		for( int i=-1;i<2;i++ ){
			for( int j=-1;j<2;j++ ){
				//if ( 0 == i && 0 == j )continue;
				s=CoordinateToIndex(x+i,y+j,n);
				if( maze[x+i][y+j] && !Visited[s] ){
					Visited[s]=true;
					path[s]=currentIndex;
					EnQueue(q,s);
				}
			}
		}
	}
		/*
		// north west
		s=CoordinateToIndex(x-1,y-1,n);
		if( maze[x-1][y-1] && !Visited[s] ){
			path[s]=i;
			Visited[s] =true;
			EnQueue(q,s);
		}
		// north
		s=CoordinateToIndex(x-1,y,n);
		if( maze[x-1][y] && !Visited[s] ){
			path[s]=i;
			Visited[s] =true;
			EnQueue(q,s);
		}
		//	north east
		s=CoordinateToIndex(x-1,y+1,n);
		if( maze[x-1][y+1] && !Visited[s] ){
			path[s]=i;
			Visited[s] =true;
			EnQueue(q,s);
		}
		//	west
		s=CoordinateToIndex(x,y-1,n);
		if( maze[x][y-1] && !Visited[s] ){
			path[s]=i;
			Visited[s] =true;
			EnQueue(q,s);
		}
		//	east
		s=CoordinateToIndex(x,y+1,n);
		if( maze[x][y+1] && !Visited[s] ){
			path[s]=i;
			Visited[s] =true;
			EnQueue(q,s);
		}
		//	south west
		s=CoordinateToIndex(x+1,y-1,n);
		if( maze[x+1][y-1] && !Visited[s] ){
			path[s]=i;
			Visited[s] =true;
			EnQueue(q,s);
		}
		//	south
		s=CoordinateToIndex(x+1,y,n);
		if( maze[x+1][y] && !Visited[s] ){
			path[s]=i;
			Visited[s] =true;
			EnQueue(q,s);
		}
		//	south east
		s=CoordinateToIndex(x+1,y+1,n);
		if( maze[x+1][y+1] && !Visited[s] ){
			path[s]=i;
			Visited[s] =true;
			EnQueue(q,s);
		}*/
	return;
}
